
import lejos.nxt.LightSensor;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.subsumption.*;
import lejos.robotics.navigation.*;


public class VirtualForces implements Behavior {
	
	static int MIN_DISTANCE_THRESHOLD = 70;
	static int MIN_ANGLE_THRESHOLD = 30;
	static int ENVIRONMENTAL_LIGHT = 40;
	static int STOP_CONDITION = 80;
	
	DifferentialPilot navigator;
	UltrasonicSensor ultrasonic;
	LightSensor lightLeft, lightRight;
	
	int[] destination;
	int[] actual;
	double bearing;
	double forceMod;
	
	public VirtualForces(DifferentialPilot nav, UltrasonicSensor ult, LightSensor ll, LightSensor lr) {
		navigator = nav;
		ultrasonic = ult;
		lightLeft = ll;
		lightRight = lr;
		
		destination = new int[]{0, 0};
		actual = new int[]{0, 0};
		
		bearing = 0;
		forceMod = 0;
	}
	
	public boolean takeControl() {

		// Sense, light and obstacles:
		int distance = ultrasonic.getDistance();
		
		System.out.println(lightRight.getLightValue()+" - "+lightLeft.getLightValue());
		
		int x = 35;
		int intensity = (lightRight.getLightValue() + lightLeft.getLightValue()) / 2;
		if (intensity > STOP_CONDITION)
			System.exit(0);
		
		int diff = lightRight.getLightValue() - lightLeft.getLightValue();
		
		if (intensity < ENVIRONMENTAL_LIGHT)
		{
			diff = 120;
		}
			
		destination = new int[]{x, diff};
		
		// Calculate the repulsion force from the ultrasonic sensor:
		int[] force;
		if (distance > MIN_DISTANCE_THRESHOLD) {
			force = new int[]{0, 0};
		} else {
			force = new int[]{-(MIN_DISTANCE_THRESHOLD - distance), 0};
		}
		
		
		// Calculate bearing angle between forces:
		actual = new int[]{destination[0] + force[0], destination[1] + force[1]};
		
		bearing = Math.atan2(actual[1], actual[0]);
		bearing = Math.toDegrees(bearing);
		
		forceMod = Math.sqrt(Math.pow(force[0], 2) + Math.pow(force[1], 2));
		
		System.out.println(bearing);
		
		// We correct the orientation if angle is large enough
		return (Math.abs(bearing) > MIN_ANGLE_THRESHOLD);
	}
	
	public void suppress() {
		
	}
	
	public void action() {
		
		
		if (Math.abs(bearing) <= 90)
		{
			//navigator.stop();
			if (bearing >= 0)
				navigator.steer(80, bearing, true);
			else
				navigator.steer(-80, bearing, true);
			//navigator.rotate(bearing);

		} 
		else
		{
			double alpha = bearing - 90;
			if (alpha > 0)
				bearing = -90  + alpha;
			else
				bearing = 90  + alpha;
			
			if (bearing >= 0 && bearing < 45)
				bearing = 45;
			else if (bearing < 0 && bearing > -45 )
				bearing = -45;
			
			double bearing1 =  bearing;
			double bearing2 = -bearing;
			Double turnRate1 = 1000*(1/forceMod);
			Double turnRate2 = -1000*(1/forceMod);		
			
			navigator.steer(turnRate1, bearing1, false);
			navigator.steer(turnRate2, bearing2, false);
		} 
			
	}

}
